﻿# coding=utf-8
# 2023/4/27: 首个记录，基于旧2020/9/28版本
# 2023/5/5: 枚举基于Enum
# 2023/10/11: 补充部分英文注释
# 2024/4/12: 修正插值时second_left/right_index未输出问题

import bi_common as bi
from math import sin, cos, atan
from enum import Enum

class LineDescription(Enum):
    MODEL = 1  # Describe with model coefficients and consistent properties / 通过模型参数和一致的属性进行描述
    SEGMENTS = 2  # Describe with many segments and properties of each segment / 通过多段线段和各自的属性进行描述
    BOTH = 3  # Both model and segments are supported / Model和Segments双模式描述


class LineClass(Enum):
    GENERAL = 1  # 未指定
    DASHED = 2  # 虚线
    SOLID = 3  # 实线
    DOUBLE_DASHED = 4  # 双虚线
    DOUBLE_SOLID = 5  # 双实线
    SOLID_DASHED = 6  # 左实右虚
    DASHED_SOLID = 7  # 左虚右实
    CURB = 8  # 路沿
    GUARDRAIL = 9  # 护栏
    BARRIER = 10  # 障碍物边界


class LineColor(Enum):
    UNKNOWN = 1  # 未知
    WHITE = 2  # 白色
    YELLOW = 3  # 黄色
    RED = 4  # 红色
    BLUE = 5  # 蓝色
    ORANGE = 6  # 橙色
    GREEN = 7  # 绿色


class LineSegmentSolidProperty:
    def __init__(self):
        self.valid = False  # Whether it's valid, False indicates virtual part / 是否有效，False表示为虚线段
        self.color = LineColor.UNKNOWN  # Color of the line segment / 线段颜色
        self.width_valid = False
        self.width = 0.0  # [m] Width of the line segment / 线段宽度


class LineSegment:
    def __init__(self):
        self.id = 0  # ID of line segment / 线段ID
        self.p1x = 0.0  # [m] x coordination of the start point / 线段首端点的x坐标
        self.p1y = 0.0  # [m] y coordination of the start point / 线段首端点的y坐标
        self.p1z_valid = False
        self.p1z = 0.0  # [m] z coordination of the start point / 线段首端点的z坐标
        self.p2x = 0.0  # [m] x coordination of the end point / 线段末端点的x坐标
        self.p2y = 0.0  # [m] y coordination of the end point / 线段末端点的y坐标
        self.p2z_valid = False
        self.p2z = 0.0  # [m] z coordination of the end point / 线段末端点的z坐标
        self.solid_property = LineSegmentSolidProperty()  # Properties of solid line segment (Not for virtual part) / 实线段的属性（虚线段则为空）


class LaneLine:
    def __init__(self):
        self.id = 0  # ID of the lane line, zero as invalid / 车道线ID，0表示无效
        self.raw_id_valid = False
        self.raw_id = 0  # Raw ID of the lane line / 车道线原始ID
        self.confidence_valid = False
        self.confidence = 0.0  # [%] Confidence of the lane line / 车道线置信度
        self.description = LineDescription.MODEL  # How to describe the lane line's curve / 车道线的描述方式
        self.classification = LineClass.GENERAL  # Model description: Classification of lane line / Model模式：一致性车道线类型
        self.color = LineColor.UNKNOWN  # Model description: Color of lane line / Model模式：一致性车道线颜色
        self.width_valid = False
        self.width = 0.0  # [m] Model description: Line width of lane line / Model模式：一致性车道线宽度
        self.param0 = 0.0  # Model description: Zero order coefficient of y-function of lane line / Model模式：车道线y方程0次方系数
        self.param1 = 0.0  # Model description: First order coefficient of y-function of lane line / Model模式：车道线y方程1次方系数
        self.param2 = 0.0  # Model description: Second order coefficient of y-function of lane line / Model模式：车道线y方程2次方系数
        self.param3 = 0.0  # Model description: Third order coefficient of y-function of lane line / Model模式：车道线y方程3次方系数
        self.paramz_valid = False  # Model description: Whether z-function is valid / Model模式：z方程是否有效
        self.paramz0 = 0.0  # Model description: Zero order coefficient of z-function of lane line / Model模式：车道线z方程0次方系数
        self.paramz1 = 0.0  # Model description: First order coefficient of z-function of lane line / Model模式：车道线z方程1次方系数
        self.paramz2 = 0.0  # Model description: Second order coefficient of z-function of lane line / Model模式：车道线z方程2次方系数
        self.paramz3 = 0.0  # Model description: Third order coefficient of z-function of lane line / Model模式：车道线z方程3次方系数
        self.rear_end_valid = False
        self.rear_end = 0.0  # [m] Model description: X-axis coordination of read end of the lane line / Model模式：曲线后端点位置(x轴)
        self.front_end_valid = False
        self.front_end = 0.0  # [m] Model description: X-axis coordination of front end of the lane line / Model模式：曲线前端点位置(x轴)
        self.segments = []  # Segment description: List of line segments / Segments模式：线段列表

    def model_valid(self):
        return self.description == LineDescription.MODEL or self.description == LineDescription.BOTH

    def segments_valid(self):
        return self.description == LineDescription.SEGMENTS or self.description == LineDescription.BOTH


class LaneSensorSample:
    def __init__(self):
        self.time = 0.0  # [s] Time offset in session / Session内的相对时间
        self.lines = []  # List of lane lines / 车道线列表
        self.first_left_index = -1  # Index of the first lane line to the left side, -1 indicated not exist / 左侧第一条车道线序号，-1表示不存在
        self.first_right_index = -1  # Index of the first lane line to the right side, -1 indicated not exist / 右侧第一条车道线序号，-1表示不存在
        self.second_left_index = -1  # Index of the second lane line to the left side, -1 indicated not exist / 左侧第二条车道线序号，-1表示不存在
        self.second_right_index = -1  # Index of the second lane line to the right side, -1 indicated not exist / 右侧第二条车道线序号，-1表示不存在
        self.lane_width_valid = False
        self.lane_width = 0.0  # [m] Current lane's width / 当前车道宽度
        self.lane_heading_valid = False
        self.lane_heading = 0.0  # [deg] Current lane's orientation / 当前车道朝向
        self.lane_curvature_valid = False
        self.lane_curvature = 0.0  # [1/m] Current lane's curvature / 当前车道曲率
        self.lateral_velocity_valid = False
        self.lateral_velocity = 0.0  # [m/s] Lateral speed to current lane / 当前车道横向速度
        self.center_departure_valid = False
        self.center_departure = 0.0  # [m] Central departure to current lane / 当前车道中央偏离距离
        self.rear_bound = 0.0  # [m] Rear bound of detect range / 传感器检测范围后边界
        self.front_bound = 30.0  # [m] Front bound of detect range / 传感器检测范围前边界
        self.vehicle_speed_valid = False
        self.vehicle_speed = 0.0  # [kph] Subject vehicle's speed / 本车车速
        self.vehicle_curvature_valid = False
        self.vehicle_curvature = 0.0  # [1/m] Subject vehicle's turning curvature / 本车行驶曲率
        self.vehicle_ax_valid = False
        self.vehicle_ax = 0.0  # [m/s²] Subject vehicle's longitudinal acceleration / 本车纵向加速度
        self.vehicle_width_valid = False
        self.vehicle_width = 0.0  # [m] Subject vehicle's width / 本车宽度
        self.vehicle_front_overhang_valid = False
        self.vehicle_front_overhang = 0.0  # [m] Subject vehicle's front overhang / 本车前悬
        self.vehicle_wheel_base_valid = False
        self.vehicle_wheel_base = 0.0  # [m] Subject vehicle's wheel base / 本车轴距

    # Calculate DTLC / 计算DTLC [m]
    def cal_dtlc(self, line):
        if not isinstance(line, LaneLine) or not line.model_valid() or not self.vehicle_width_valid or not self.vehicle_front_overhang_valid:
            return None
        is_left = line.param0 > 0
        x = -self.vehicle_front_overhang
        y = (line.param3 * x * x * x + line.param2 * x * x + line.param1 * x + line.param0) * (1 if is_left else -1)
        cos_theta = abs(cos(atan(line.param1)))
        return cos_theta * (y - self.vehicle_width * 0.5)

    # Calculate STLC / 计算STLC [m/s]
    def cal_stlc(self, line):
        if not isinstance(line, LaneLine) or not line.model_valid() or not self.vehicle_speed_valid:
            return None
        is_left = line.param0 > 0
        curv = self.vehicle_curvature if self.vehicle_curvature_valid else 0
        fo = self.vehicle_front_overhang if self.vehicle_front_overhang_valid else 0.9
        wb = self.vehicle_wheel_base if self.vehicle_wheel_base_valid else 2.8
        sin_theta = sin(atan(line.param1 - curv * (fo + wb)))
        return (-1 if is_left else 1) * sin_theta * self.vehicle_speed / 3.6

    # Calculate TTLC / 计算TTLC [s]
    def cal_ttlc(self, line):
        if not isinstance(line, LaneLine) or not line.model_valid() or not self.vehicle_speed_valid or not self.vehicle_width_valid or not self.vehicle_front_overhang_valid:
            return None
        if self.vehicle_speed <= 0:
            return None
        is_left = line.param0 > 0
        wb = self.vehicle_wheel_base if self.vehicle_wheel_base_valid else 2.8
        x0 = -self.vehicle_front_overhang - wb
        x = x0
        y = 0
        vx = self.vehicle_speed / 3.6
        loop_times = 1000 if vx < 5 else int(50.0 / vx * 100)  # No output if crossing beyond 50m (Can't guarantee precision) / 超过50米越线则不输出（精度无法保证）
        rx = wb
        ry = (0.5 if is_left else -0.5) * self.vehicle_width
        for i in range(0, loop_times + 1):
            k = (self.vehicle_curvature * (x - x0)) if self.vehicle_curvature_valid else 0
            cosa = 1.0 / abs(1 + k)
            sina = cosa * k
            tx = x + cosa * rx - sina * ry
            ty = y + sina * rx + cosa * ry
            ly = line.param3 * tx * tx * tx + line.param2 * tx * tx + line.param1 * tx + line.param0
            passed = False
            if is_left:
                if ly <= ty:
                    passed = True
            else:
                if ly >= ty:
                    passed = True
            if passed:
                if i == 0:
                    return None
                else:
                    return 0.01 * i
            if self.vehicle_ax_valid:
                vx = vx + 0.01 * self.vehicle_ax
            if vx <= 0:
                break
            x = x + vx * 0.01 * cosa
            y = y + vx * 0.01 * sina
        return None

    # Convert to general sample for output / 转通用样本，用于样本输出
    def to_general_sample(self, channel):
        output = bi.agency.create_general_sample()
        output.protocol = "lane-sensor-sample-v4@" + str(channel)
        segment_count = 0
        segment_offsets = []
        segment_sizes = []
        for line in self.lines:
            segment_offsets.append(segment_count)
            segment_sizes.append(len(line.segments))
            segment_count += len(line.segments)
        output.time = self.time
        output.significant = 23 + len(self.lines) * 27 + segment_count * 12
        output.values = []
        output.values.append(len(self.lines))
        output.values.append(segment_count)
        output.values.append(self.lateral_velocity if self.lateral_velocity_valid else None)
        output.values.append(self.center_departure if self.center_departure_valid else None)
        output.values.append(self.lane_width if self.lane_width_valid else None)
        output.values.append(self.lane_heading if self.lane_heading_valid else None)
        output.values.append(self.lane_curvature if self.lane_curvature_valid else None)
        output.values.append(self.rear_bound)
        output.values.append(self.front_bound)
        output.values.append(self.first_left_index if self.first_left_index >= 0 else None)
        output.values.append(self.first_right_index if self.first_right_index >= 0 else None)
        output.values.append(self.second_left_index if self.second_left_index >= 0 else None)
        output.values.append(self.second_right_index if self.second_right_index >= 0 else None)
        output.values.append(self.vehicle_speed if self.vehicle_speed_valid else None)
        output.values.append(self.vehicle_curvature if self.vehicle_curvature_valid else None)
        output.values.append(self.vehicle_ax if self.vehicle_ax_valid else None)
        output.values.append(self.vehicle_width if self.vehicle_width_valid else None)
        output.values.append(self.vehicle_front_overhang if self.vehicle_front_overhang_valid else None)
        output.values.append(self.vehicle_wheel_base if self.vehicle_wheel_base_valid else None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        output.values.append(None)
        i = 0
        for line in self.lines:
            output.values.append(line.id)
            output.values.append(line.raw_id if line.raw_id_valid else None)
            output.values.append(line.confidence if line.confidence_valid else None)
            output.values.append(line.description.value)
            if line.model_valid():
                output.values.append(line.classification.value)
                output.values.append(line.color.value)
                output.values.append(line.width if line.width_valid else None)
                output.values.append(line.param0)
                output.values.append(line.param1)
                output.values.append(line.param2)
                output.values.append(line.param3)
                output.values.append(1 if line.paramz_valid else 0)
                output.values.append(line.paramz0)
                output.values.append(line.paramz1)
                output.values.append(line.paramz2)
                output.values.append(line.paramz3)
                output.values.append(line.rear_end if line.rear_end_valid else None)
                output.values.append(line.front_end if line.front_end_valid else None)
            else:
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
                output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            output.values.append(None)
            if line.segments_valid():
                output.values.append(segment_offsets[i])
                output.values.append(segment_sizes[i])
            else:
                output.values.append(None)
                output.values.append(None)
            i += 1
        return output


def _conv_lane_sensor_sample_v3(gs):
    values_count = len(gs.values)
    if values_count < 17:
        return None
    line_count = int(gs.values[0]) if gs.values[0] is not None else 0
    if values_count != 17 + line_count * 15:
        return None
    output = LaneSensorSample()
    output.time = gs.time
    if gs.values[1] is not None:
        output.lateral_velocity_valid = True
        output.lateral_velocity = float(gs.values[1])
    if gs.values[2] is not None:
        output.center_departure_valid = True
        output.center_departure = float(gs.values[2])
    if gs.values[3] is not None:
        output.lane_width_valid = True
        output.lane_width = float(gs.values[3])
    if gs.values[4] is not None:
        output.lane_heading_valid = True
        output.lane_heading = float(gs.values[4])
    if gs.values[5] is not None:
        output.lane_curvature_valid = True
        output.lane_curvature = float(gs.values[5])
    if gs.values[6] is not None:
        output.vehicle_speed_valid = True
        output.vehicle_speed = float(gs.values[6])
    if gs.values[7] is not None:
        output.vehicle_curvature_valid = True
        output.vehicle_curvature = float(gs.values[7])
    if gs.values[8] is not None:
        output.vehicle_ax_valid = True
        output.vehicle_ax = float(gs.values[8])
    if gs.values[9] is not None:
        output.vehicle_width_valid = True
        output.vehicle_width = float(gs.values[9])
    if gs.values[10] is not None:
        output.vehicle_front_overhang_valid = True
        output.vehicle_front_overhang = float(gs.values[10])
    output.rear_bound = float(gs.values[11]) if gs.values[11] is not None else 0.0
    output.front_bound = float(gs.values[12]) if gs.values[12] is not None else 30.0
    output.first_left_index = int(gs.values[13]) if gs.values[13] is not None else -1
    output.first_right_index = int(gs.values[14]) if gs.values[14] is not None else -1
    output.second_left_index = int(gs.values[15]) if gs.values[15] is not None else -1
    output.second_right_index = int(gs.values[16]) if gs.values[16] is not None else -1
    for i in range(0, line_count):
        line = LaneLine()
        b = 17 + 15 * i
        line.id = int(gs.values[b] if gs.values[b] is not None else 0)
        if gs.values[b + 1] is not None:
            line.raw_id_valid = True
            line.raw_id = int(gs.values[b + 1])
        line.classification = LineClass(int(gs.values[b + 2])) if gs.values[b + 2] is not None else LineClass.GENERAL
        line.color = LineColor(int(gs.values[b + 3])) if gs.values[b + 3] is not None else LineColor.UNKNOWN
        if gs.values[b + 4] is not None:
            line.confidence_valid = True
            line.confidence = float(gs.values[b + 4])
        if gs.values[b + 5] is not None:
            line.width_valid = True
            line.width = float(gs.values[b + 5])
        line.param0 = float(gs.values[b + 6]) if gs.values[b + 6] is not None else 0.0
        line.param1 = float(gs.values[b + 7]) if gs.values[b + 7] is not None else 0.0
        line.param2 = float(gs.values[b + 8]) if gs.values[b + 8] is not None else 0.0
        line.param3 = float(gs.values[b + 9]) if gs.values[b + 9] is not None else 0.0
        if gs.values[b + 10] is not None:
            line.rear_end_valid = True
            line.rear_end = float(gs.values[b + 10])
        if gs.values[b + 11] is not None:
            line.front_end_valid = True
            line.front_end = float(gs.values[b + 11])
        output.lines.append(line)
    return output


def _conv_lane_sensor_sample_v4(gs):
    values_count = len(gs.values)
    if values_count < 23:
        return None
    line_count = int(gs.values[0]) if gs.values[0] is not None else 0
    segment_count = int(gs.values[1]) if gs.values[1] is not None else 0
    if values_count != 23 + line_count * 27 + segment_count * 12:
        return None
    output = LaneSensorSample()
    output.time = gs.time
    if gs.values[2] is not None:
        output.lateral_velocity_valid = True
        output.lateral_velocity = float(gs.values[2])
    if gs.values[3] is not None:
        output.center_departure_valid = True
        output.center_departure = float(gs.values[3])
    if gs.values[4] is not None:
        output.lane_width_valid = True
        output.lane_width = float(gs.values[4])
    if gs.values[5] is not None:
        output.lane_heading_valid = True
        output.lane_heading = float(gs.values[5])
    if gs.values[6] is not None:
        output.lane_curvature_valid = True
        output.lane_curvature = float(gs.values[6])
    output.rear_bound = float(gs.values[7]) if gs.values[7] is not None else 0.0
    output.front_bound = float(gs.values[8]) if gs.values[8] is not None else 30.0
    output.first_left_index = int(gs.values[9]) if gs.values[9] is not None else -1
    output.first_right_index = int(gs.values[10]) if gs.values[10] is not None else -1
    output.second_left_index = int(gs.values[11]) if gs.values[11] is not None else -1
    output.second_right_index = int(gs.values[12]) if gs.values[12] is not None else -1
    if gs.values[13] is not None:
        output.vehicle_speed_valid = True
        output.vehicle_speed = float(gs.values[13])
    if gs.values[14] is not None:
        output.vehicle_curvature_valid = True
        output.vehicle_curvature = float(gs.values[14])
    if gs.values[15] is not None:
        output.vehicle_ax_valid = True
        output.vehicle_ax = float(gs.values[15])
    if gs.values[16] is not None:
        output.vehicle_width_valid = True
        output.vehicle_width = float(gs.values[16])
    if gs.values[17] is not None:
        output.vehicle_front_overhang_valid = True
        output.vehicle_front_overhang = float(gs.values[17])
    if gs.values[18] is not None:
        output.vehicle_wheel_base_valid = True
        output.vehicle_wheel_base = float(gs.values[18])
    seg_base = 23 + 27 * line_count
    for i in range(0, line_count):
        line = LaneLine()
        b = 23 + 27 * i
        line.id = int(gs.values[b] if gs.values[b] is not None else 0)
        if gs.values[b + 1] is not None:
            line.raw_id_valid = True
            line.raw_id = int(gs.values[b + 1])
        if gs.values[b + 2] is not None:
            line.confidence_valid = True
            line.confidence = float(gs.values[b + 2])
        line.description = LineDescription(int(gs.values[b + 3])) if gs.values[b + 3] is not None else LineDescription.MODEL
        if line.model_valid():
            line.classification = LineClass(int(gs.values[b + 4])) if gs.values[b + 4] is not None else LineClass.GENERAL
            line.color = LineColor(int(gs.values[b + 5])) if gs.values[b + 5] is not None else LineColor.UNKNOWN
            if gs.values[b + 6] is not None:
                line.width_valid = True
                line.width = float(gs.values[b + 6])
            line.param0 = float(gs.values[b + 7]) if gs.values[b + 7] is not None else 0.0
            line.param1 = float(gs.values[b + 8]) if gs.values[b + 8] is not None else 0.0
            line.param2 = float(gs.values[b + 9]) if gs.values[b + 9] is not None else 0.0
            line.param3 = float(gs.values[b + 10]) if gs.values[b + 10] is not None else 0.0
            line.paramz_valid = gs.values[b + 11] == 1 if gs.values[b + 11] is not None else False
            line.paramz0 = float(gs.values[b + 12]) if gs.values[b + 12] is not None else 0.0
            line.paramz1 = float(gs.values[b + 13]) if gs.values[b + 13] is not None else 0.0
            line.paramz2 = float(gs.values[b + 14]) if gs.values[b + 14] is not None else 0.0
            line.paramz3 = float(gs.values[b + 15]) if gs.values[b + 15] is not None else 0.0
            if gs.values[b + 16] is not None:
                line.rear_end_valid = True
                line.rear_end = float(gs.values[b + 16])
            if gs.values[b + 17] is not None:
                line.front_end_valid = True
                line.front_end = float(gs.values[b + 17])
        if line.segments_valid():
            seg_offset = int(gs.values[b + 25])
            seg_size = int(gs.values[b + 26])
            for n in range(0, seg_size):
                seg = LineSegment()
                seg_b = seg_base + 12 * (seg_offset + n)
                seg.id = int(gs.values[seg_b]) if gs.values[seg_b] is not None else 0
                seg.p1x = float(gs.values[seg_b + 1]) if gs.values[seg_b + 1] is not None else 0.0
                seg.p1y = float(gs.values[seg_b + 2]) if gs.values[seg_b + 2] is not None else 0.0
                if gs.values[seg_b + 3] is not None:
                    seg.p1z_valid = True
                    seg.p1z = float(gs.values[seg_b + 3])
                seg.p2x = float(gs.values[seg_b + 4]) if gs.values[seg_b + 4] is not None else 0.0
                seg.p2y = float(gs.values[seg_b + 5]) if gs.values[seg_b + 5] is not None else 0.0
                if gs.values[seg_b + 6] is not None:
                    seg.p2z_valid = True
                    seg.p2z = float(gs.values[seg_b + 6])
                seg.solid_property.valid = gs.values[seg_b + 7] == 1 if gs.values[seg_b + 7] is not None else False
                if seg.solid_property.valid is True:
                    seg.solid_property.color = int(gs.values[seg_b + 8]) if gs.values[seg_b + 8] is not None else LineColor.UNKNOWN
                    if gs.values[seg_b + 9] is not None:
                        seg.solid_property.width_valid = True
                        seg.solid_property.width = float(gs.values[seg_b + 9])
                line.segments.append(seg)
        output.lines.append(line)
    return output


def _interpolate_lane_sensor_sample(s1, w1, s2, w2):
    output = LaneSensorSample()
    output.time = bi.time
    if s1.lateral_velocity_valid and s2.lateral_velocity_valid:
        output.lateral_velocity_valid = True
        output.lateral_velocity = s1.lateral_velocity * w1 + s2.lateral_velocity * w2
    if s1.center_departure_valid and s2.center_departure_valid:
        output.center_departure_valid = True
        output.center_departure = s1.center_departure * w1 + s2.center_departure * w2
    if s1.lane_width_valid and s2.lane_width_valid:
        output.lane_width_valid = True
        output.lane_width = s1.lane_width * w1 + s2.lane_width * w2
    if s1.lane_heading_valid and s2.lane_heading_valid:
        output.lane_heading_valid = True
        output.lane_heading = s1.lane_heading * w1 + s2.lane_heading * w2
    if s1.lane_curvature_valid and s2.lane_curvature_valid:
        output.lane_curvature_valid = True
        output.lane_curvature = s1.lane_curvature * w1 + s2.lane_curvature * w2
    output.rear_bound = s1.rear_bound if w1 > w2 else s2.rear_bound
    output.front_bound = s1.front_bound if w1 > w2 else s2.front_bound
    output.vehicle_width_valid = s1.vehicle_width_valid
    output.vehicle_width = s1.vehicle_width
    output.vehicle_wheel_base_valid = s1.vehicle_wheel_base_valid
    output.vehicle_wheel_base = s1.vehicle_wheel_base
    output.vehicle_front_overhang_valid = s1.vehicle_front_overhang_valid
    output.vehicle_front_overhang = s1.vehicle_front_overhang
    if s1.vehicle_speed_valid and s2.vehicle_speed_valid:
        output.vehicle_speed_valid = True
        output.vehicle_speed = s1.vehicle_speed * w1 + s2.vehicle_speed * w2
    if s1.vehicle_curvature_valid and s2.vehicle_curvature_valid:
        output.vehicle_curvature_valid = True
        output.vehicle_curvature = s1.vehicle_curvature * w1 + s2.vehicle_curvature * w2
    if s1.vehicle_ax_valid and s2.vehicle_ax_valid:
        output.vehicle_ax_valid = True
        output.vehicle_ax = s1.vehicle_ax * w1 + s2.vehicle_ax * w2
    first_left_id = -1
    if s1.first_left_index >= 0 and s2.first_left_index >= 0 and s1.lines[s1.first_left_index].id == s2.lines[s2.first_left_index].id:
        first_left_id = s1.lines[s1.first_left_index].id
    first_right_id = -1
    if s1.first_right_index >= 0 and s2.first_right_index >= 0 and s1.lines[s1.first_right_index].id == s2.lines[s2.first_right_index].id:
        first_right_id = s1.lines[s1.first_right_index].id
    second_left_id = -1
    if s1.second_left_index >= 0 and s2.second_left_index >= 0 and s1.lines[s1.second_left_index].id == s2.lines[s2.second_left_index].id:
        second_left_id = s1.lines[s1.second_left_index].id
    second_right_id = -1
    if s1.second_right_index >= 0 and s2.second_right_index >= 0 and s1.lines[s1.second_right_index].id == s2.lines[s2.second_right_index].id:
        second_right_id = s1.lines[s1.second_right_index].id
    index = 0
    for o1 in s1.lines:
        id = o1.id
        for o2 in s2.lines:
            if o2.id != id:
                continue
            if first_left_id == id:
                output.first_left_index = index
            if first_right_id == id:
                output.first_right_index = index
            if second_left_id == id:
                output.second_left_index = index
            if second_right_id == id:
                output.second_right_index = index
            line = LaneLine()
            line.id = id
            line.raw_id_valid = o1.raw_id_valid
            line.raw_id = o1.raw_id
            if o1.confidence_valid and o2.confidence_valid:
                line.confidence_valid = True
                line.confidence = float(o1.confidence * w1 + o2.confidence * w2)
            line.description = o1.description
            if line.model_valid():
                line.classification = o1.classification if w1 > w2 else o2.classification
                line.color = o1.color if w1 > w2 else o2.color
                if o1.width_valid and o2.width_valid:
                    line.width_valid = True
                    line.width = float(o1.width * w1 + o2.width * w2)
                line.param0 = o1.param0 * w1 + o2.param0 * w2
                line.param1 = o1.param1 * w1 + o2.param1 * w2
                line.param2 = o1.param2 * w1 + o2.param2 * w2
                line.param3 = o1.param3 * w1 + o2.param3 * w2
                if o1.paramz_valid and o2.paramz_valid:
                    line.paramz_valid = True
                    line.paramz0 = o1.paramz0 * w1 + o2.paramz0 * w2
                    line.paramz1 = o1.paramz1 * w1 + o2.paramz1 * w2
                    line.paramz2 = o1.paramz2 * w1 + o2.paramz2 * w2
                    line.paramz3 = o1.paramz3 * w1 + o2.paramz3 * w2
                if o1.rear_end_valid and o2.rear_end_valid:
                    line.rear_end_valid = True
                    line.rear_end = float(o1.rear_end * w1 + o2.rear_end * w2)
                if o1.front_end_valid and o2.front_end_valid:
                    line.front_end_valid = True
                    line.front_end = float(o1.front_end * w1 + o2.front_end * w2)
            if line.segments_valid():
                for seg1 in o1.segments:
                    sid = seg1.id
                    for seg2 in o2.segments:
                        if seg2.id != id:
                            continue
                        seg = LineSegment()
                        seg.id = sid
                        seg.p1x = seg1.p1x * w1 + seg2.p1x * w2
                        seg.p1y = seg1.p1y * w1 + seg2.p1y * w2
                        if seg1.p1z_valid and seg2.p1z_valid:
                            seg.p1z_valid = True
                            seg.p1z = seg1.p1z * w1 + seg2.p1z * w2
                        seg.p2x = seg1.p2x * w1 + seg2.p2x * w2
                        seg.p2y = seg1.p2y * w1 + seg2.p2y * w2
                        if seg1.p2z_valid and seg2.p2z_valid:
                            seg.p2z_valid = True
                            seg.p2z = seg1.p2z * w1 + seg2.p2z * w2
                        seg.solid_property = seg1.solid_property if w1 > w2 else seg2.solid_property
                        line.segments.append(seg)
            output.lines.append(line)
            index += 1
    return output


# Query LaneSensorSample for input / 获取LaneSensorSample，用于样本输入
def get_lane_sensor_sample(channel):
    s1 = None
    s2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id_v3 = 'lane-sensor-sample-v3@' + str(channel)
    protocol_id_v4 = 'lane-sensor-sample-v4@' + str(channel)
    if protocol_id_v4 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v4]
        s1 = _conv_lane_sensor_sample_v4(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_lane_sensor_sample_v4(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v3 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v3]
        s1 = _conv_lane_sensor_sample_v3(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_lane_sensor_sample_v3(pair.sample2)
        w2 = pair.weight2
    if s1 is not None and s2 is not None:
        return _interpolate_lane_sensor_sample(s1, w1, s2, w2)
    return None
